
#include "leg.h"
#include "math.h"
#include "matlab_motion.h"
#include "usr_conf.h"
Leg_t leg[4];


void Leg_Init()
{
	for(int i = 0; i < 4; i++)
	{
		leg[i].zero_y = LEG_ZEROY;
	}
	// leg[LEG_INDEX_RB].zero_y += 0.1;
}

void Leg_SetOutputMax(Leg_t *leg, float spd_max_out, float pos_max_out)
{
	leg->m1.spd_pid.outputMax = leg->m2.spd_pid.outputMax = spd_max_out;	
	leg->m1.pos_pid.outputMax = leg->m2.pos_pid.outputMax = pos_max_out;	
}

//void Leg_SetZeroAngle(Leg_index_e leg_index, float zero_angle)
//{
//	leg[leg_index].zero_angle = zero_angle;
//}
void Leg_SetAngle(Leg_index_e leg_index, float degree1, float degree2)
{
	switch(leg_index)
	{
		case LEG_INDEX_LF:
		{
			degree1 = degree1;
			degree2 = degree2;
			
			break;
		}
		case LEG_INDEX_RF:
		{
			degree1 = degree1;
			degree2 = degree2;
			break;
		}
		case LEG_INDEX_LB:
		{
			degree1 = degree1;
			degree2 = degree2;
			break;
		}
		case LEG_INDEX_RB:
		{
			degree1 = degree1;
			degree2 = degree2;
			break;
		}
	}
	Motor_SetPos(&leg[leg_index].m1, degree1);
	Motor_SetPos(&leg[leg_index].m2, degree2);
}

//x正向为狗前进方向
//y正向朝下

//左前腿选alpha[1],theta[1]
//左后腿内部电机alpha，于是编号为1
//左后腿外部电机theta，于是编号为2


//左后腿选alpha[0],theta[0]
//左前腿纵置电机alpha，于是编号为1
//左前腿横置电机theta，于是编号为2

// void Leg_SetPos(Leg_index_e leg_index, float x, float y)
// {
// 	if(x*x + y*y > 23*23 || x*x + y*y < 11.5*11.5 || y < 0) return; //限幅
// 	if(leg_index == LEG_INDEX_LF || leg_index == LEG_INDEX_LB ) x = -x;
// 	float lx = leg[leg_index].x, ly = leg[leg_index].y;
// 	if(sqrt((lx - x)*(lx - x) + (ly - y)+(ly - y)) > 0.8) 
// 	{
// 		float p = 0.4;
// 		leg[leg_index].x = x*p + leg[leg_index].x*(1-p);
// 		leg[leg_index].y = y*p + leg[leg_index].y*(1-p);
// 	}
// 	else 
// 	{
// 		leg[leg_index].x = x*0.5 + leg[leg_index].x*0.5;
// 		leg[leg_index].y = y*0.5 + leg[leg_index].y*0.5;
// 	}
// 	leg[leg_index].x = x;
// 	leg[leg_index].y = y;
// 	char i0;
	
// 	static double alpha[2], theta[2];

// 	float angles[4];
	
// 	matlab_motion_solve(leg[leg_index].x, leg[leg_index].y, angles);
// 	// matlab_motion_solve(x, y, angles);
	
	
// 	angles[0] = angles[0]-PI;
// 	angles[3] = angles[3]-PI;
	
// 	alpha[0] = angles[0];//
// 	alpha[1] = angles[1];
	
// 	//规格化到-pi到pi
// 	for(i0 = 0; i0 < 2; i0++)
// 	{
// 		while(alpha[i0] > PI) alpha[i0] -= 2*PI;
// 		while(alpha[i0] < -PI) alpha[i0] += 2*PI;
// 	}
	
// 	theta[0] = angles[2];
// 	theta[1] = angles[3];
	
// 	//规格化到-pi到pi
// 	for(i0 = 0; i0 < 2; i0++)
// 	{
// 		while(theta[i0] > PI) theta[i0] -= 2*PI;
// 		while(theta[i0] < -PI) theta[i0] += 2*PI;
// 	}
	
// 	switch(leg_index)
// 	{
// 		case LEG_INDEX_LF:
// 			Leg_SetAngle(leg_index, RAD_2_DEG(alpha[1]), RAD_2_DEG(theta[1]));
// 			break;
// 		case LEG_INDEX_RF:
// 			Leg_SetAngle(leg_index, RAD_2_DEG(alpha[1]), RAD_2_DEG(theta[1]));
			
// 			break;
// 		case LEG_INDEX_LB:
// 			Leg_SetAngle(leg_index, RAD_2_DEG(alpha[1]), RAD_2_DEG(theta[1]));
			
// 			break;
// 		case LEG_INDEX_RB:
// 			Leg_SetAngle(leg_index, RAD_2_DEG(alpha[0]), RAD_2_DEG(theta[0]));
			
// 			break;
// 	}
	
// }

void Leg_SetPos(Leg_index_e leg_index, float x, float y)
{
	if(x*x + y*y > 23*23 || x*x + y*y < 11.5*11.5 || y < 0) return; //限幅
	
	leg[leg_index].x = x;
	leg[leg_index].y = y;
	if(leg_index == LEG_INDEX_LF || leg_index == LEG_INDEX_LB ) x = -x;
	char i0;
	
	static double alpha[2], theta[2];

	float angles[4];
	
	matlab_motion_solve(x, y, angles);
	
	
	angles[0] = angles[0]-PI;
	angles[3] = angles[3]-PI;
	
	alpha[0] = angles[0];//
	alpha[1] = angles[1];
	
	//规格化到-pi到pi
	for(i0 = 0; i0 < 2; i0++)
	{
		while(alpha[i0] > PI) alpha[i0] -= 2*PI;
		while(alpha[i0] < -PI) alpha[i0] += 2*PI;
	}
	
	theta[0] = angles[2];
	theta[1] = angles[3];
	
	//规格化到-pi到pi
	for(i0 = 0; i0 < 2; i0++)
	{
		while(theta[i0] > PI) theta[i0] -= 2*PI;
		while(theta[i0] < -PI) theta[i0] += 2*PI;
	}
	
	switch(leg_index)
	{
		case LEG_INDEX_RB:
			Leg_SetAngle(leg_index, RAD_2_DEG(alpha[0]), RAD_2_DEG(theta[0]));
			break;
		case LEG_INDEX_RF:
			Leg_SetAngle(leg_index, RAD_2_DEG(alpha[1]), RAD_2_DEG(theta[1]));
			
			break;
		case LEG_INDEX_LB:
			Leg_SetAngle(leg_index, RAD_2_DEG(alpha[1]), RAD_2_DEG(theta[1]));
			
			break;
		case LEG_INDEX_LF:
			Leg_SetAngle(leg_index, RAD_2_DEG(alpha[0]), RAD_2_DEG(theta[0]));
			
			break;
	}
	
}

